#include <stdlib.h>

#include <util/mem.h>
#include <util/math_macros.h>
#include <util/msg.h>
#include <util/common_macros.h>

#include "../../dsp/primitives.h"
#include "../../dsp/filters/filters.h"
#include "features.h"


// =============================================================================
/**
 * Calculates Dx, Dy and Dxy derivatives of a matrix
 * 
 * This function uses an optimal derivative filter described by Farid and Simoncelli.
 * To best results we use a 5x5 derivative filter.
 * 
 * @param Mat input matrix
 * @param Dx gradient matrix in horizontal (x) direction
 * @param Dy gradient matrix in vertical (y) direction
 * @param Dxy gradient matrix in vertical direction of dx
 */
void imp_matrix_derivative(ImpMatrix * Mat, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Dxy) {

	ImpMatrix P = imp_filter_farid_simoncelli(5, 'x', 1);
	ImpMatrix D1 = imp_filter_farid_simoncelli(5, 'x', 2);

	if (*Dx == NULL) (*Dx) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dy == NULL) (*Dy) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dxy == NULL) (*Dxy) = imp_matrix_new(Mat->rows, Mat->columns);

	convolve2D_separable(Mat->data, Mat->rows, Mat->columns, P.data, P.size, D1.data, D1.size, (*Dx)->data);
	convolve2D_separable(Mat->data, Mat->rows, Mat->columns, D1.data, D1.size, P.data, P.size, (*Dy)->data);
	convolve2D_separable((*Dx)->data, (*Dx)->rows, (*Dx)->columns, D1.data, D1.size, P.data, P.size, (*Dxy)->data);

}
/**
 * Calculates Dx, Dy and Dxy derivatives of a matrix using a mask.
 * 
 * The points where the Mask value is 0 will not be calculated.
 * 
 * This function uses an optimal derivative filter described by Farid and Simoncelli.
 * To best results we use a 5x5 derivative filter.
 * 
 * @param Mat input matrix
 * @param Mask input mask
 * @param Dx gradient matrix in horizontal (x) direction
 * @param Dy gradient matrix in vertical (y) direction
 * @param Dxy gradient matrix in vertical direction of dx
 */
void imp_matrix_derivative_with_mask(ImpMatrix * Mat, ImpMatrix * Mask, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Dxy) {

	ImpMatrix Fx = imp_filter_farid_simoncelli_5("dx", IMP_FILTER_FULL);
	ImpMatrix Fy = imp_filter_farid_simoncelli_5("dy", IMP_FILTER_FULL);
	ImpMatrix Fxy = imp_filter_farid_simoncelli_5("dxy", IMP_FILTER_FULL);

	int i = 0;
	if (i) {
		imp_matrix_print(&Fx);
		imp_matrix_print(&Fy);
	}

	if (*Dx == NULL) (*Dx) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dy == NULL) (*Dy) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dxy == NULL) (*Dxy) = imp_matrix_new(Mat->rows, Mat->columns);

	convolve2D_border_reflect_fast(Mat->data, Mat->rows, Mat->columns, Fx.data, Fx.rows, Fx.columns, (*Dx)->data);
	convolve2D_border_reflect_fast_with_mask(Mat->data, Mat->rows, Mat->columns, Fy.data, Fy.rows, Fy.columns, Mask->data, (*Dy)->data);
	convolve2D_border_reflect_fast_with_mask((*Dx)->data, Mat->rows, Mat->columns, Fy.data, Fy.rows, Fy.columns, Mask->data, (*Dxy)->data);

}
/**
 * Calculates Dxx and Dyy derivatives of a matrix
 * 
 * This function uses an optimal derivative filter described by Farid and Simoncelli.
 * To best results we use a 5x5 derivative filter.
 * 
 * @param Mat input matrix
 * @param Dxx 2st derivative matrix in horizontal (x) direction
 * @param Dyy 2st derivative matrix in vertical (y) direction
 */
void imp_matrix_derivative2(ImpMatrix * Mat, ImpMatrix ** Dxx, ImpMatrix ** Dyy) {

	ImpMatrix P = imp_filter_farid_simoncelli(5, 'x', 1);
	ImpMatrix D2 = imp_filter_farid_simoncelli_2st(5, 'x', 2);

	if (*Dxx == NULL) (*Dxx) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dyy == NULL) (*Dyy) = imp_matrix_new(Mat->rows, Mat->columns);

	convolve2D_separable(Mat->data, Mat->rows, Mat->columns, P.data, P.size, D2.data, D2.size, (*Dxx)->data);
	convolve2D_separable(Mat->data, Mat->rows, Mat->columns, D2.data, D2.size, P.data, P.size, (*Dyy)->data);
}
/**
 * Calculates Dxx and Dyy derivatives of a matrix using a mask.
 * 
 * The points where the Mask value is 0 will not be calculated.
 * 
 * This function uses an optimal derivative filter described by Farid and Simoncelli.
 * To best results we use a 5x5 derivative filter.
 * 
 * @param Mat input matrix
 * @param Mask input mask
 * @param Dxx 2st derivative matrix in horizontal (x) direction
 * @param Dyy 2st derivative matrix in vertical (y) direction
 */
void imp_matrix_derivative2_with_mask(ImpMatrix * Mat, ImpMatrix * Mask, ImpMatrix ** Dxx, ImpMatrix ** Dyy) {

	ImpMatrix Fxx = imp_filter_farid_simoncelli_5("dxx", IMP_FILTER_FULL);
	ImpMatrix Fyy = imp_filter_farid_simoncelli_5("dyy", IMP_FILTER_FULL);

	int i = 0;
	if (i) {
		imp_matrix_print(&Fxx);
		imp_matrix_print(&Fyy);
	}

	if (*Dxx == NULL) (*Dxx) = imp_matrix_new(Mat->rows, Mat->columns);
	if (*Dyy == NULL) (*Dyy) = imp_matrix_new(Mat->rows, Mat->columns);

	convolve2D_border_reflect_fast_with_mask(Mat->data, Mat->rows, Mat->columns, Fxx.data, Fxx.rows, Fxx.columns, Mask->data, (*Dxx)->data);
	convolve2D_border_reflect_fast_with_mask(Mat->data, Mat->rows, Mat->columns, Fyy.data, Fyy.rows, Fyy.columns, Mask->data, (*Dyy)->data);
}
/**
 * \brief This function calculates the Hessian response: H = Dxx*Dyy - Dxy*Dxy of a matrix
 * 
 * @param Mat input matrix
 * @param Resp Hessian response matrix
 */
void imp_image_features_hessian_response(ImpMatrix * Mat, ImpMatrix ** Resp) {

	if (*Resp == NULL) (*Resp) = imp_matrix_new(Mat->rows, Mat->columns);

	ImpMatrix * Dx = NULL;
	ImpMatrix * Dy = NULL;
	ImpMatrix * Dxy = NULL;

	ImpMatrix * Dxx = NULL;
	ImpMatrix * Dyy = NULL;

	imp_matrix_derivative(Mat, &Dx, &Dy, &Dxy);
	imp_matrix_derivative2(Mat, &Dxx, &Dyy);

	int i;
	for (i = 0; i < Mat->size; i++) {
		(*Resp)->data[i] = (Dxx->data[i] * Dyy->data[i]) - (Dxy->data[i] * Dxy->data[i]);
		//(*Resp)->data[i] = -(*Resp)->data[i];
	}

	imp_object_delete(Dx);
	imp_object_delete(Dy);
	imp_object_delete(Dxy);
	imp_object_delete(Dxx);
	imp_object_delete(Dyy);
}
/**
 * \brief This function calculates the Hessian response: H = Dxx*Dyy - Dxy*Dxy of a matrix
 * using a mask.
 * 
 * The Hessian response is calculated only on point where the Mask is different of 0.
 * 
 * @param Mat input matrix
 * @param Mask input matrix
 * @param Resp Hessian response matrix
 */
void imp_image_features_hessian_response_with_mask(ImpMatrix * Mat, ImpMatrix * Mask, ImpMatrix ** Resp) {

	if (*Resp == NULL) (*Resp) = imp_matrix_new(Mat->rows, Mat->columns);

	ImpMatrix * Dx = NULL;
	ImpMatrix * Dy = NULL;
	ImpMatrix * Dxy = NULL;

	ImpMatrix * Dxx = NULL;
	ImpMatrix * Dyy = NULL;

	imp_matrix_derivative_with_mask(Mat, Mask, &Dx, &Dy, &Dxy);
	//imp_matrix_derivative(Mat, &Dx, &Dy, &Dxy);
	imp_matrix_derivative2_with_mask(Mat, Mask, &Dxx, &Dyy);
	//imp_matrix_derivative2(Mat, &Dxx, &Dyy);

	int i;
	for (i = 0; i < Mat->size; i++) {
		(*Resp)->data[i] = (Dxx->data[i] * Dyy->data[i]) - (Dxy->data[i] * Dxy->data[i]);
		//(*Resp)->data[i] *= (*Resp)->data[i];
		//(*Resp)->data[i] = ABS((*Resp)->data[i]);
	}

	imp_object_delete(Dx);
	imp_object_delete(Dy);
	imp_object_delete(Dxy);
	imp_object_delete(Dxx);
	imp_object_delete(Dyy);
}